package mdp.algorithm.explore;

import mdp.algorithm.simulator.*;
import mdp.algorithm.explore.Sensors;
import mdp.algorithm.fastestPath.PathSolver;
import mdp.rpi.communicator.RaspberryController;

import java.awt.Point;
//import java.math.BigInteger;


import javax.swing.JLabel;

public class Robot implements Runnable {
	
	private static final int perCoverage = 100;
	private int counter = 0;
	private int virtualMap[][];
	private int robotOrientation;
	private int robotMidPointRow, robotMidPointCol; //Robot's current position
	
	//private float minimumCoverage = 0.8f;
	
	private boolean exploreMode = true, goal = false, start;
	
	private String command, lastCommand;
	private Sensors sensors;
	private Stopwatch stopwatch;
	private RaspberryController rpiInterface;
	private JLabel exploreTimeValue;
	
	private int terminateCounter = 00;
	private int terminateCounterPosition = -1; 
	private int specialCounter = 0;
	
	
	private static final String C_ARDUINO = "HP:", C_ASTART = "A:XP", C_ANDROID = "AP:",
								C_ASOLVE="A:XS", C_APOLL = "A:GRID", C_LEFT = "L", C_RIGHT = "R", 
								C_180 = "B", C_ADVANCE = "F", C_START="X", C_CALIBRATE = "C", 
								C_STOP = "S";
	private static final String WARNING_LABEL = "Warning:\t";
	
	private static final int BITS = 13;
	private static final int MAP_ROW = 22, MAP_COL = 17;
	private static final int NORTH = 0, EAST = 1, SOUTH = 2, WEST = 3;
	private static final int UNKNOWN = 0, EXPLORED = 1, OBSTACLES = 2, ROBOT_POSITION = 3 ;
	private static final int DEFAULT_ROBOT_MIDPOINT_ROW = 11, DEFAULT_ROBOT_MIDPOINT_COL = 8, DEFAULT_ORIENTATION = WEST;
	
	private static int callibrateCounter = 0; 
	
	private Maze maze;
	
	public Robot(Maze maze, JLabel exploreTimeValue, String timeLimit, float minimumCoverage) {
		start = true;
		this.maze = maze;
		//this.minimumCoverage = minimumCoverage;
		this.exploreTimeValue = exploreTimeValue;
		//maxSearchTime = Integer.parseInt(timeLimit.substring(0, 1)) + (Float.parseFloat(timeLimit.substring(2)) * 1/60);
		sensors = new Sensors();

		initRobot(DEFAULT_ROBOT_MIDPOINT_ROW, DEFAULT_ROBOT_MIDPOINT_COL, DEFAULT_ORIENTATION);
	}
	
	/*****************
	 *   ALGORITHM   *
	 *****************/
	public void explore() {
		try {
			Thread.sleep(500);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		if(!goal)
			goal = isRobotAtGoal();	
		if(start) {
			if (robotMidPointRow == 2 || robotMidPointRow == 19 || robotMidPointCol == 2 || robotMidPointCol == 14) {
				start = false;
				spinLeft();
			} else {
				if (counter == 0) {
					if (isFrontClear())
						moveForward();
					else {
						spinLeft();
						counter--;
					}
				} else {
					if (isRightClear()) {
						spinRight();
						if (isFrontClear())
							moveForward();
						counter++;
					} else if (isFrontClear()) {
						moveForward();
					} else {
						spinLeft();
						if(isFrontClear()) {
							moveForward();
						}
						counter--;
					}
				}
			}
		}
		else {
			callibrateCounter++;
			if(callibrateCounter >= 5 && onRightWall()){
				callibrateCounter = 0;
				calibrate();
			}
			else{
				if (isRightClear()) {
					spinRight();
					if(isFrontClear())
						moveForward();
				} else if (isFrontClear())
					moveForward();
				else if (isLeftClear()) {
					spinLeft();
					if (isFrontClear())
						moveForward();
				} else {
					spin180();
					if (isFrontClear()) {
						moveForward();
					}
				}
			}
		}
		//ALOYSIUS THIS ONE NEED?
		robotMidPointRow = maze.getRobotMidPointRow();
		robotMidPointCol = maze.getRobotMidPointCol();	
		
		if(checkGoal()){
			if(terminateCounter%10 == 0)
				terminateCounter++;
			if(terminateCounter == 11 && terminateCounterPosition==-1)
				terminateCounterPosition = 0;
		}
			
		
		if(checkStart()){
			if(terminateCounter < 10)
				terminateCounter+=10;
			if(terminateCounter == 11 && terminateCounterPosition==-1)
				terminateCounterPosition = 1;
		}
	}
	private boolean terminateOneRound(){
		if((terminateCounterPosition == 0 && checkStart()) || (terminateCounterPosition == 1 && checkGoal())){
			//System.out.println("TERMINATE HEREEEEEEEEEEEEEEEEEEEEEEEEEE");
			return true;
		}
		return false;
	}
	
	private boolean onRightWall(){
		switch(robotOrientation){
		case 0: 
			if(maze.checkObstacle(robotMidPointRow-1, robotMidPointCol+2) && maze.checkObstacle(robotMidPointRow, robotMidPointCol+2) && maze.checkObstacle(robotMidPointRow+1, robotMidPointCol+2)){
				return true;
			}
			else 
				return false;
		case 1:
			if(maze.checkObstacle(robotMidPointRow+2, robotMidPointCol-1) && maze.checkObstacle(robotMidPointRow+2, robotMidPointCol) && maze.checkObstacle(robotMidPointRow+2, robotMidPointCol+1)){
				return true;
			}
			else 
				return false;
		
		case 2:
			if(maze.checkObstacle(robotMidPointRow-1, robotMidPointCol-2) && maze.checkObstacle(robotMidPointRow, robotMidPointCol-2) && maze.checkObstacle(robotMidPointRow+1, robotMidPointCol-2)){
				return true;
			}
			else 
				return false;
		case 3:
			if(maze.checkObstacle(robotMidPointRow-2, robotMidPointCol-1) && maze.checkObstacle(robotMidPointRow-2, robotMidPointCol) && maze.checkObstacle(robotMidPointRow-2, robotMidPointCol+1)){
				return true;
			}
			else 
				return false;
		}
		return false;
		
	}
	
	private void calibrate() {
		rpiInterface.sendMessage(C_ARDUINO + C_CALIBRATE);
		updateTelemetricInformation(telemetricPolling());
	}

	private boolean checkGoal(){
		if(robotMidPointRow==2 && robotMidPointCol == 14)
			return true;
		return false;
	}
	
	private boolean checkStart(){
		if(robotMidPointRow==19 && robotMidPointCol == 2)
			return true;
		return false;
	}
	
	/* UNTOUCHED */
	private void stopExplore() {
		//rpiInterface.sendMessage(C_ARDUINO+C_STOP);
		exploreMode = false;
		String combinedString="", movementCommand="";
		stopwatch.stopStopwatchTimer();
		Point start = new Point(maze.getRobotMidPointRow(), maze.getRobotMidPointCol());
		
		if((maze.checkExplored(2,13)|| maze.checkExplored(3,14)) && maze.checkExplored(2,14) && !checkStart()){
        	PathSolver ps1 = new PathSolver();
        	Point goal1 = new Point(19,2);
    		combinedString = ps1.moveRobot(ps1.solvePath(start,goal1,maze,maze.getRobotOrientation(), rpiInterface),start,goal1,maze,maze.getRobotOrientation());
        }
		System.out.println("Robot is now attempting to backtrack to the starting position.");
		for (specialCounter = 0; specialCounter < combinedString.length(); specialCounter++) {
			movementCommand = combinedString.substring(specialCounter, specialCounter+1);
			if (movementCommand.equalsIgnoreCase(C_STOP))
				break;
			else {
				switch (movementCommand) {
					case C_180: spin180();
						break;
					case C_ADVANCE: moveForward();
						break;
					case C_LEFT: spinLeft();
						break;
					case C_RIGHT: spinRight();
				}
			}
		}
		
        while(true) {
        	System.out.println("Waiting for Android to issue FASTEST PATH command.");
			command = rpiInterface.listen();
			if (!command.isEmpty()) {
				if (command.toUpperCase().equals(C_ASOLVE)) {
					break;
				} else if (command.toUpperCase().contains(C_APOLL)) {
					command = C_ANDROID + maze.androidMapDesc2();
					rpiInterface.sendMessage(command.toUpperCase());
				}
				else
					System.out.println(WARNING_LABEL + "PC is not receiving the correct data from RPi.\n"
							+ "\t\tWaiting for fastest path command from Android.");
			}
			command = "";
        }
		
        //go to start
		Point goal2 = new Point(2,14);
        PathSolver ps2 = new PathSolver();
		combinedString = ps2.moveRobot(ps2.solvePath(start,goal2,maze,maze.getRobotOrientation(), rpiInterface),start,goal2,maze,maze.getRobotOrientation());
		System.out.println("Robot is now performing a Fastest Path! Congratulations Team!");
		for (specialCounter = 0; specialCounter < combinedString.length(); specialCounter++) {
			movementCommand = combinedString.substring(specialCounter, specialCounter+1);
			if (movementCommand.equalsIgnoreCase(C_STOP))
				break;
			else {
				switch (movementCommand) {
					case C_180: spin180();
						break;
					case C_ADVANCE: moveForward();
						break;
					case C_LEFT: spinLeft();
						break;
					case C_RIGHT: spinRight();
				}
			}
		}
		maze.completePath();
		
	}
	/*END UNTOUCH*/
	
	/*********************************************
	 *   DO NOT MAKE ANY CHANGES FROM HERE ON.   *
	 *********************************************/
	/* Robot Memory Access */
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	/*private int mapCoverage() {
		int gridsCovered = 0;
		for (int i = 1; i < MAP_ROW-1; i++)
			for (int j = 1; j < MAP_COL-1; j++) {
				if (virtualMap[i][j] != 0)
					gridsCovered++;
			}
		return gridsCovered;
	}*/
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	/*private double mapCoveredPercentage() {
		if (mapCoverage() == 0)
			return 0;
		return mapCoverage()/(MAP_ROW*MAP_COL);
	}*/
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private boolean isRobotAtGoal(){
		if(robotMidPointRow==2 && robotMidPointCol == 14)
			return true;
		return false;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void printVirtualMap() {
		System.out.println("\n");
		for (int row = 0; row < MAP_ROW; row++) {
			for (int col = 0; col < MAP_COL; col++)
				System.out.print(virtualMap[row][col] + "	");
			System.out.println("\n");
		}
	}
	
	/* Telemetric Manager */
	private String telemetricPolling() {
		String telemetricCommand = "";
		while (telemetricCommand == "") {
			System.out.println("Subsequent Polling for Telemetric Information.");
			telemetricCommand = rpiInterface.listen();
			if (!telemetricCommand.isEmpty()) {
				if (!telemetricCommand.startsWith("A:") && telemetricCommand.length() == BITS) {
					break;
				} else if (telemetricCommand.toUpperCase().contains(C_APOLL)) {
					command = C_ANDROID + maze.androidMapDesc2();
					rpiInterface.sendMessage(command.toUpperCase());
					telemetricCommand = "";
				} else if (telemetricCommand.toUpperCase().contains(C_ASTART)) {
					//command = C_ARDUINO+C_START;
					rpiInterface.sendMessage(lastCommand.toUpperCase());
					telemetricCommand = "";
				} else {
					System.out.println(WARNING_LABEL + "PC is not receiving the correct data from RPi.\n"
							+ "\t\tWaiting for telemetric information from Robot.");
					telemetricCommand = "";
				}
			}
		}
		return telemetricCommand;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void updateTelemetricInformation(String syntax) {
		String data;
		//Detected Obstacles on Left
		System.out.println("Updating Telemetric Information");
		if (exploreMode) {
			if (!syntax.contains(C_APOLL)) {
				data = syntax.substring(0,1);
				sensors.setLeftObstacles(0, (data.equals("1") ? true : false));
				data = syntax.substring(1,2);
				sensors.setLeftObstacles(1, (data.equals("1") ? true : false));
				data = syntax.substring(2,3);
				sensors.setLeftObstacles(2, (data.equals("1") ? true : false));
				data = syntax.substring(3,4);
				sensors.setLeftObstacles(3, (data.equals("1") ? true : false));
				data = syntax.substring(4,5);
				sensors.setLeftObstacles(4, (data.equals("1") ? true : false));
				
				//Detected Obstacles on Right
				data = syntax.substring(5,6);
				sensors.setRightObstacles(0, (data.equals("1") ? true : false));
				data = syntax.substring(6,7);
				sensors.setRightObstacles(1, (data.equals("1") ? true : false));
				data = syntax.substring(7,8);
				sensors.setRightObstacles(2, (data.equals("1") ? true : false));
				data = syntax.substring(8,9);
				sensors.setRightObstacles(3, (data.equals("1") ? true : false));
				data = syntax.substring(9,10);
				
				//Detected Obstacles in Front
				data = syntax.substring(10,11);
				System.out.println("FX " + (data.equals("1") ? "has obstacles.\n" : "has no obstacles.\n"));
				sensors.setFrontObstacles(0, (data.equals("1") ? true : false));
				data = syntax.substring(11,12);
				System.out.println("FY " + (data.equals("1") ? "has obstacles.\n" : "has no obstacles.\n"));
				sensors.setFrontObstacles(1, (data.equals("1") ? true : false));
				data = syntax.substring(12,13);
				System.out.println("FZ " + (data.equals("1") ? "has obstacles.\n" : "has no obstacles.\n"));
				sensors.setFrontObstacles(2, (data.equals("1") ? true : false));
				
				//Update Virtual Map		
				for (int i = 0; i < sensors.getFrontSensorRow().length; i++) {
					if (sensors.getFrontSensorRow(i)< MAP_ROW && sensors.getFrontSensorRow(i) >= 0 && sensors.getFrontSensorCol(i) < MAP_COL && sensors.getFrontSensorCol(i) >= 0) {
						if (sensors.getFrontSensorRow(i) == 0 || sensors.getFrontSensorRow(i) == MAP_ROW-1 || sensors.getFrontSensorCol(i) == 0 || sensors.getFrontSensorCol(i) == MAP_COL-1) {
							sensors.setFrontObstacles(i, true);
						} else if (sensors.getFrontObstacles(i)) {
							virtualMap[sensors.getFrontSensorRow(i)][sensors.getFrontSensorCol(i)] = OBSTACLES;
						} else {
							virtualMap[sensors.getFrontSensorRow(i)][sensors.getFrontSensorCol(i)] = EXPLORED;
						}
					}
				}
		
				//Left Sensor
				for (int i = 0; i < sensors.getLeftSensorRow().length; i++) {
					if (sensors.getLeftSensorRow(i) < MAP_ROW && sensors.getLeftSensorRow(i) >= 0 && sensors.getLeftSensorCol(i) < MAP_COL && sensors.getLeftSensorCol(i) >= 0) {
						
						if (((sensors.getLeftSensorRow(i) == 1 || sensors.getLeftSensorRow(i) == 2 || sensors.getLeftSensorRow(i) == 3) && (sensors.getLeftSensorCol(i) == 13 || sensors.getLeftSensorCol(i) == 14 || sensors.getLeftSensorCol(i) == 15))) {
							sensors.setLeftObstacles(i, false);
							System.out.println("Left Sensor Location in Goal Zone");
						} else if (((sensors.getLeftSensorRow(i) == 18 || sensors.getLeftSensorRow(i) == 19 || sensors.getLeftSensorRow(i) == 20) && (sensors.getLeftSensorCol(i) == 1 || sensors.getLeftSensorCol(i) == 2 || sensors.getLeftSensorCol(i) == 3))) {
							sensors.setLeftObstacles(i, false);
							System.out.println("Left Sensor Location in Start Zone");
						} else if (virtualMap[sensors.getLeftSensorRow(i)][sensors.getLeftSensorCol(i)] == OBSTACLES) {
							sensors.setLeftObstacles(i, true);
							System.out.println("Obstacle Location on Left Side: " + i);
							break;
						} else if(sensors.getLeftObstacles(i)) {
							virtualMap[sensors.getLeftSensorRow(i)][sensors.getLeftSensorCol(i)] = OBSTACLES;
							System.out.println("Obstacle Location on Left Side: " + i);
							break;
						} else {
							virtualMap[sensors.getLeftSensorRow(i)][sensors.getLeftSensorCol(i)] = EXPLORED;
						}
					} else 
						break;
				}
				
				//Right Sensor
				for (int i = 0; i < sensors.getRightSensorRow().length; i++) {
					if (sensors.getRightSensorRow(i) < MAP_ROW && sensors.getRightSensorRow(i) >= 0 && sensors.getRightSensorCol(i) < MAP_COL && sensors.getRightSensorCol(i) >= 0) {
						if (((sensors.getRightSensorRow(i) == 1 || sensors.getRightSensorRow(i) == 2 || sensors.getRightSensorRow(i) == 3) && (sensors.getRightSensorCol(i) == 13 || sensors.getRightSensorCol(i) == 14 || sensors.getRightSensorCol(i) == 15))) {
							sensors.setRightObstacles(i, false);
							System.out.println("Right Sensor Location in Goal Zone");
						} else if (((sensors.getRightSensorRow(i) == 18 || sensors.getRightSensorRow(i) == 19 || sensors.getRightSensorRow(i) == 20) && (sensors.getRightSensorCol(i) == 1 || sensors.getRightSensorCol(i) == 2 || sensors.getRightSensorCol(i) == 3))) {
							sensors.setRightObstacles(i, false);
							System.out.println("Right Sensor Location in Start Zone");
						} else if (virtualMap[sensors.getRightSensorRow(i)][sensors.getRightSensorCol(i)] == OBSTACLES) {
							sensors.setRightObstacles(i, true);
							System.out.println("Obstacle Location on Right Side: " + i);
							break;
						} else if(sensors.getRightObstacles(i)) {
							virtualMap[sensors.getRightSensorRow(i)][sensors.getRightSensorCol(i)] = OBSTACLES;
							System.out.println("Obstacle Location on Right Side: " + i);
							break;
						} else {
							virtualMap[sensors.getRightSensorRow(i)][sensors.getRightSensorCol(i)] = EXPLORED;
						}
					} else 
						break;
				}
				
				maze.plotMap(sensors);
			}
		} else {
			if (!syntax.isEmpty()) {
				if (syntax.toUpperCase().equals(C_ASTART)) {
					specialCounter--;
				}
			}
		}
	}
	
	/* Robot's Action */
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void moveForward() {
		switch(robotOrientation) {
		case NORTH:
			if(robotMidPointRow > 2){
				robotMidPointRow--;
				//Set Trail
				virtualMap[robotMidPointRow+2][robotMidPointCol-1] = EXPLORED;
				virtualMap[robotMidPointRow+2][robotMidPointCol] = EXPLORED;
				virtualMap[robotMidPointRow+2][robotMidPointCol+1] = EXPLORED;
				
				//Set Next Grid
				virtualMap[robotMidPointRow-1][robotMidPointCol-1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow-1][robotMidPointCol] = ROBOT_POSITION;
				virtualMap[robotMidPointRow-1][robotMidPointCol+1] = ROBOT_POSITION;
			}
			break;
		case EAST:
			if(robotMidPointCol < 14){
				robotMidPointCol++;
				//Set Trail
				virtualMap[robotMidPointRow-1][robotMidPointCol-2] = EXPLORED;
				virtualMap[robotMidPointRow][robotMidPointCol-2] = EXPLORED;
				virtualMap[robotMidPointRow+1][robotMidPointCol-2] = EXPLORED;
				
				//Set Next Grid
				virtualMap[robotMidPointRow-1][robotMidPointCol+1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow][robotMidPointCol+1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow+1][robotMidPointCol+1] = ROBOT_POSITION; 
			}
			break;
		case SOUTH:
			if(robotMidPointRow < 19){
				robotMidPointRow++;
				//Set Trail
				virtualMap[robotMidPointRow-2][robotMidPointCol-1] = EXPLORED;
				virtualMap[robotMidPointRow-2][robotMidPointCol] = EXPLORED;
				virtualMap[robotMidPointRow-2][robotMidPointCol+1] = EXPLORED;
				
				//Set Next Grid
				virtualMap[robotMidPointRow+1][robotMidPointCol-1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow+1][robotMidPointCol] = ROBOT_POSITION;
				virtualMap[robotMidPointRow+1][robotMidPointCol+1] = ROBOT_POSITION;
			}
			break;
		case WEST:
			if(robotMidPointCol > 2){
				robotMidPointCol--;
				//Set Trail
				virtualMap[robotMidPointRow-1][robotMidPointCol+2] = EXPLORED;
				virtualMap[robotMidPointRow][robotMidPointCol+2] = EXPLORED;
				virtualMap[robotMidPointRow+1][robotMidPointCol+2] = EXPLORED;
				
				//Set Next Grid
				virtualMap[robotMidPointRow-1][robotMidPointCol-1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow][robotMidPointCol-1] = ROBOT_POSITION;
				virtualMap[robotMidPointRow+1][robotMidPointCol-1] = ROBOT_POSITION;
			}
			break;
		}
		maze.moveForwardAction();
		initSensorLocations();
		lastCommand = command = C_ARDUINO + C_ADVANCE;
		rpiInterface.sendMessage(command);
		updateTelemetricInformation(telemetricPolling());
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void spinLeft() {
		maze.spinLeftAction();
		robotOrientation = (robotOrientation + 3) % 4;
		initSensorLocations();
		lastCommand = command = C_ARDUINO + C_LEFT;
		rpiInterface.sendMessage(command);
		updateTelemetricInformation(telemetricPolling());
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void spinRight() {
		maze.spinRightAction();
		robotOrientation = (robotOrientation + 1) % 4;
		initSensorLocations();
		lastCommand = command = C_ARDUINO + C_RIGHT;
		rpiInterface.sendMessage(command);
		updateTelemetricInformation(telemetricPolling());
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void spin180() {
		maze.spinRightAction();
		maze.spinRightAction();
		robotOrientation = (robotOrientation + 2) % 4;
		initSensorLocations();
		lastCommand = command = C_ARDUINO + C_180;
		rpiInterface.sendMessage(command);
		updateTelemetricInformation(telemetricPolling());
	}
	
	/* Check Surrounding*/
	public boolean isFrontClear(){
		switch(robotOrientation){
		case 0: //NORTH
			if(virtualMap[robotMidPointRow-2][robotMidPointCol+1]!=OBSTACLES && virtualMap[robotMidPointRow-2][robotMidPointCol]!=OBSTACLES && virtualMap[robotMidPointRow-2][robotMidPointCol-1]!=OBSTACLES)
				return true;
			break;
		case 1: //EAST
			if(virtualMap[robotMidPointRow+1][robotMidPointCol+2]!=OBSTACLES && virtualMap[robotMidPointRow][robotMidPointCol+2]!=OBSTACLES && virtualMap[robotMidPointRow-1][robotMidPointCol+2]!=OBSTACLES)
				return true;
			break;
		case 2: //SOUTH
			if(virtualMap[robotMidPointRow+2][robotMidPointCol-1]!=OBSTACLES && virtualMap[robotMidPointRow+2][robotMidPointCol]!=OBSTACLES && virtualMap[robotMidPointRow+2][robotMidPointCol+1]!=OBSTACLES)
				return true;
			break;
		case 3: //WEST
			if(virtualMap[robotMidPointRow-1][robotMidPointCol-2]!=OBSTACLES && virtualMap[robotMidPointRow][robotMidPointCol-2]!=OBSTACLES && virtualMap[robotMidPointRow+1][robotMidPointCol-2]!=OBSTACLES)
				return true;
			break;
		}
		return false;
	}
	
	private boolean isLeftClear() {
		switch(robotOrientation){
		case 0: //NORTH
			if(virtualMap[robotMidPointRow-1][robotMidPointCol-2]!=OBSTACLES && virtualMap[robotMidPointRow][robotMidPointCol-2]!=OBSTACLES && virtualMap[robotMidPointRow+1][robotMidPointCol-2]!=OBSTACLES)
				return true;
			break;
		case 1: //EAST
			if(virtualMap[robotMidPointRow-2][robotMidPointCol+1]!=OBSTACLES && virtualMap[robotMidPointRow-2][robotMidPointCol]!=OBSTACLES && virtualMap[robotMidPointRow-2][robotMidPointCol-1]!=OBSTACLES)
				return true;
			break;
		case 2: //SOUTH
			if(virtualMap[robotMidPointRow+1][robotMidPointCol+2]!=OBSTACLES && virtualMap[robotMidPointRow][robotMidPointCol+2]!=OBSTACLES && virtualMap[robotMidPointRow-1][robotMidPointCol+2]!=OBSTACLES)
				return true;
			break;
		case 3: //WEST
			if(virtualMap[robotMidPointRow+2][robotMidPointCol-1]!=OBSTACLES && virtualMap[robotMidPointRow+2][robotMidPointCol]!=OBSTACLES && virtualMap[robotMidPointRow+2][robotMidPointCol+1]!=OBSTACLES)
				return true;
			break;
		}
		return false;
	}
	
	private boolean isRightClear() {
		switch(robotOrientation){
		case NORTH:
			if(virtualMap[robotMidPointRow-1][robotMidPointCol+2]!=OBSTACLES && virtualMap[robotMidPointRow][robotMidPointCol+2]!=OBSTACLES && virtualMap[robotMidPointRow+1][robotMidPointCol+2]!=OBSTACLES)
				return true;
			break;
		case EAST:
			if(virtualMap[robotMidPointRow+2][robotMidPointCol+1]!=OBSTACLES && virtualMap[robotMidPointRow+2][robotMidPointCol]!=OBSTACLES && virtualMap[robotMidPointRow+2][robotMidPointCol-1]!=OBSTACLES)
				return true;
			break;
		case SOUTH:
			if(virtualMap[robotMidPointRow+1][robotMidPointCol-2]!=OBSTACLES && virtualMap[robotMidPointRow][robotMidPointCol-2]!=OBSTACLES && virtualMap[robotMidPointRow-1][robotMidPointCol-2]!=OBSTACLES)
				return true;
			break;
		case WEST:
			if(virtualMap[robotMidPointRow-2][robotMidPointCol-1]!=OBSTACLES && virtualMap[robotMidPointRow-2][robotMidPointCol]!=OBSTACLES && virtualMap[robotMidPointRow-2][robotMidPointCol+1]!=OBSTACLES)
				return true;
			break;
		}
		return false;
	}

	/* Loading Robot's Component */
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void initRobot(int startRow, int startCol, int robotOrientation) {
		maze.setRobotMidPoint(startRow, startCol);
		robotMidPointRow = startRow;
		robotMidPointCol = startCol;
		this.robotOrientation = robotOrientation;
		maze.setRobotOrientation(robotOrientation);
		maze.setRobotMidPoint(startRow, startCol);
		initSensorLocations();
		System.out.println("Sensors Activated.");
		
		initVirtualMap();
		System.out.println("Virtual Map Initialised.");
		
		initStopwatch();
		System.out.println("Stopwatch Initialised.");
		
		if (initRaspberryConnection())
			System.out.println("Connection with Raspberry Established.");
		else {
			System.out.println("Attempting to reconnect.");
			if (initRaspberryConnection())
				System.out.println("Connection with Raspberry Established.");
			else {
				System.out.println("We are unable to establish a communication link to RPi.");
				return;
			}
		}
		System.out.println("---------- All Systems Ready to be Deployed. ----------");
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void initVirtualMap() {
		virtualMap = new int[MAP_ROW][MAP_COL];
		
		for (int row = 0; row < MAP_ROW; row++) {
			for (int col = 0; col < MAP_COL; col++) {
				if (row == 0 || row == MAP_ROW-1 || col == 0 || col == MAP_COL-1)
					virtualMap[row][col] = OBSTACLES;
				else
					virtualMap[row][col] = UNKNOWN;
			}
		}
		
		virtualMap[robotMidPointRow-1][robotMidPointCol-1] = ROBOT_POSITION;
		virtualMap[robotMidPointRow-1][robotMidPointCol] = ROBOT_POSITION;
		virtualMap[robotMidPointRow-1][robotMidPointCol+1] = ROBOT_POSITION;
		
		virtualMap[robotMidPointRow][robotMidPointCol-1] = ROBOT_POSITION;
		virtualMap[robotMidPointRow][robotMidPointCol] = ROBOT_POSITION;
		virtualMap[robotMidPointRow][robotMidPointCol+1] = ROBOT_POSITION;
		
		virtualMap[robotMidPointRow+1][robotMidPointCol-1] = ROBOT_POSITION;
		virtualMap[robotMidPointRow+1][robotMidPointCol] = ROBOT_POSITION;
		virtualMap[robotMidPointRow+1][robotMidPointCol+1] = ROBOT_POSITION;
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void initSensorLocations() {
		int row, col;
		switch(robotOrientation) {
			case NORTH:	
					row = robotMidPointRow - 2;
					col = robotMidPointCol;
					sensors.setFrontSensors(row, col-1, row, col, row, col+1);
					row = robotMidPointRow - 1;
					col = robotMidPointCol - 2;
					sensors.setLeftSensors(row, col, row, col - 1, row, col - 2, row, col - 3,row, col - 4);
					col = robotMidPointCol + 2;
					sensors.setRightSensors(row, col, row, col + 1, row, col + 2, row, col + 3, row, col + 4);
					break;
			case EAST:	
					row = robotMidPointRow;
					col = robotMidPointCol + 2;
					sensors.setFrontSensors(row-1, col, row, col, row+1, col);
					row = robotMidPointRow - 2;
					col = robotMidPointCol + 1;
					sensors.setLeftSensors(row, col, row - 1, col, row - 2, col, row - 3, col, row - 4, col);
					row = robotMidPointRow + 2;
					sensors.setRightSensors(row, col, row + 1, col, row + 2, col, row + 3, col, row + 4, col);
					break;
			case SOUTH:	
					row = robotMidPointRow + 2;
					col = robotMidPointCol;
					sensors.setFrontSensors(row, col+1, row, col, row, col-1);
					row = robotMidPointRow + 1;
					col = robotMidPointCol + 2;
					sensors.setLeftSensors(row, col, row, col + 1, row, col + 2, row, col + 3, row, col + 4);
					col = robotMidPointCol - 2;
					sensors.setRightSensors(row, col, row, col - 1, row, col - 2, row, col - 3, row, col - 4);
					break;
			case WEST:
					col = robotMidPointCol - 2;
					row = robotMidPointRow;
					sensors.setFrontSensors(row+1, col, row, col, row-1, col);
					col = robotMidPointCol-1;
					row = robotMidPointRow + 2;
					sensors.setLeftSensors(row, col, row + 1, col, row + 2, col, row + 3, col, row + 4, col);
					row = robotMidPointRow - 2;
					sensors.setRightSensors(row,col, row - 1, col, row - 2, col, row - 3, col, row - 4, col);
					break;
		}
		System.out.println("Updated FX: " + sensors.getFxRow() + ", " + sensors.getFxCol());
		System.out.println("Updated FY: " + sensors.getFyRow() + ", " + sensors.getFyCol());
		System.out.println("Updated FZ: " + sensors.getFzRow() + ", " + sensors.getFzCol());
		System.out.println("Sensory Locations Updated.");
	}	
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private void initStopwatch() {
		stopwatch = new Stopwatch();
	}
	/* SYNTAX CONFIRMED BY ALOYSIUS */
	private boolean initRaspberryConnection() {
		try {
			rpiInterface = new RaspberryController();
			return true;
		} catch (Exception e) {
			System.out.println(WARNING_LABEL + "connection to Raspberry Pi has timeout.");
			return false;
		}
	}

	@Override
	public void run() {
		//rpiInterface.sendMessage("XS:BEGIN");

		while(true) {
			System.out.println("Waiting for Android's Command.");
			command = rpiInterface.listen();
			if (!command.isEmpty()) {
				if (command.toUpperCase().equals(C_ASTART)) {
					command = C_ARDUINO+C_START;
					rpiInterface.sendMessage(command.toUpperCase());
					stopwatch.startStopwatchTimer(exploreTimeValue);
					break;
				} else if (command.toUpperCase().equals(C_APOLL)) {
					command = C_ANDROID + maze.androidMapDesc2();
					rpiInterface.sendMessage(command.toUpperCase());
				}
				else
					System.out.println(WARNING_LABEL + "PC is not receiving the correct data from RPi.\n"
							+ "\t\tWaiting for a START command from Android.");
			}
			command = "";
		}
		command = "";
		while(true) {
			System.out.println("First Time Polling for Telemetric Information.");
			command = rpiInterface.listen();
			if (!command.isEmpty()) {
				if (command.toUpperCase().contains(C_APOLL)) {
					command = C_ANDROID + maze.androidMapDesc2();
					rpiInterface.sendMessage(command.toUpperCase());
				} else if (command.toUpperCase().equals(C_ASTART)) {
					command = C_ARDUINO+C_START;
					rpiInterface.sendMessage(command.toUpperCase());
				} else if (!command.startsWith("A:") && command.length() == BITS) {
					updateTelemetricInformation(command);
					break;
				} else
					System.out.println(WARNING_LABEL + "PC is not receiving the correct data from RPi.\n"
							+ "\t\tWaiting for telemetric information from Robot.");
			}
			command = "";
		}
		printVirtualMap();
		while((maze.percentageExplored() < perCoverage && !stopwatch.getCurrentTime().contains("5:")) && !terminateOneRound()) {
			System.out.println("---------------------------------------------------------------------------");
			maze.esSetExplorationPathAction();
			explore();
			printVirtualMap();
			System.out.println(stopwatch.getCurrentTime());
		}
		rpiInterface.closeCommLog();
		maze.esSetExplorationPathAction();
		stopExplore();
	}
	
}
